﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using KinectAdapter;
using Microsoft.Kinect;

namespace KinectAdapter
{
    public class SteeringWheel : KinectForkliftAdapter
    {
        public override event EventHandler StopIssued = null;
        public override event EventHandler MenuIssued = null;

        private bool speedSteeringOn = false;
        private bool turnSteeringOn = false;
        private bool forkSteeringOn = false;

        private const double SPEED_STEERING_HANDS_POS_DIFF_MIN_X = 0.3;
        private const double SPEED_STEERING_HANDS_POS_DIFF_MAX_X = 0.5;
        private const double SPEED_STEERING_HANDS_POS_DIFF_MIN_YZ = 0.0;
        private const double SPEED_STEERING_HANDS_POS_DIFF_MAX_YZ = 0.1;

        private const double TURN_STEERING_HANDS_POS_DIFF_MIN_XY = 0.3;
        private const double TURN_STEERING_HANDS_POS_DIFF_MAX_XY = 0.5;
        private const double TURN_STEERING_HANDS_POS_DIFF_MIN_Z = 0.0;
        private const double TURN_STEERING_HANDS_POS_DIFF_MAX_Z = 0.15;

        private const double FORK_STEERING_HANDS_POS_DIFF_MIN_X = 0.0;
        private const double FORK_STEERING_HANDS_POS_DIFF_MAX_X = 0.2;
        private const double FORK_STEERING_HANDS_POS_DIFF_MIN_YZ = 0.0;
        private const double FORK_STEERING_HANDS_POS_DIFF_MAX_YZ = 0.1;

        private const double LIMB_SIZE = 0.55;
        private const double FORK_SPEED = 0.005;
        private const double FORK_STOP_ZONE = 0.1;

        private const double TURN_SNAP_TO_ZERO_HANDS_POS_DIF = 0.01;
        private const double TURN_SCALE = 1.7;

        private const double ROBOT_SPEED = 0.005;
        private const double ACCELERATION_STOP_ZONE = 0.15;
        private const double ACCELERATION_STOP_ZONE_AREA = 0.05;
        private const double ACCELERATION_ZONE = ACCELERATION_STOP_ZONE + ACCELERATION_STOP_ZONE_AREA;
        private const double DEACCELERATION_ZONE = ACCELERATION_STOP_ZONE - ACCELERATION_STOP_ZONE_AREA;

        private const double X_DATA_SCALE = 2.0;
        private const double Y_DATA_SCALE = 1.5;
        private const double Z_DATA_SCALE = 4.0;
        private const double X_ZERO_RANGE = 0.15;
        private const double Y_ZERO_RANGE = 0.15;
        private const double Z_ZERO_RANGE = 0.15;

        /// <summary>
        /// Initiates the adapter.
        /// Start() methods needs to be called for it to start working.
        /// </summary>
        public SteeringWheel()
        {
            X = new KinectSlider() { Min = -1, Max = 1, Scale = X_DATA_SCALE, ZeroRange = X_ZERO_RANGE};
            Y = new KinectSlider() { Min = -1, Max = 1, Scale = Y_DATA_SCALE, ZeroRange = Y_ZERO_RANGE };
            Z = new KinectSlider() { Min = -1, Max = 1, Scale = Z_DATA_SCALE, ZeroRange = Z_ZERO_RANGE };
        }

        /// <summary>
        /// Handles the skeleton data and changes the values of control.
        /// Also detects gestures.
        /// </summary>
        /// <param name="skeleton">Skeleton data received.</param>
        protected override void GetSkeletonData(Skeleton skeleton)
        {
            speedSteeringOn = CheckSpeedSteering(skeleton);
            turnSteeringOn = CheckTurnSteering(skeleton);
            forkSteeringOn = CheckForkSteering(skeleton);

            MainSteeringOn = CheckMainSteering(skeleton);
            SecondarySteeringOn = CheckSecondarySteering(skeleton);

            if (!firstFrame)
            {
                if (forkSteeringOn)
                {
                    double handsPosDiff = skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.Spine].Position.Y;

                    if (Math.Abs(handsPosDiff) <= FORK_STOP_ZONE)
                    {
                        // hands are in stop zone
                        // do not change value
                    }
                    else if (skeleton.Joints[JointType.HandRight].Position.Y >= skeleton.Joints[JointType.Spine].Position.Y)
                    {
                        // fork up
                        Y.ChangeValue(FORK_SPEED);
                    }
                    else
                    {
                        // fork down
                        Y.ChangeValue(-FORK_SPEED);
                    }
                }

                if (speedSteeringOn)
                {
                    double distance = skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.Spine].Position.Y;

                    if (Math.Abs(distance) >= ACCELERATION_ZONE)
                    {
                        // speed up
                        Z.ChangeValue(ROBOT_SPEED);

                    }
                    else if (Math.Abs(distance) <= DEACCELERATION_ZONE)
                    {
                        // slow down
                        Z.ChangeValue(-ROBOT_SPEED);

                    }
                    else
                    {
                        // keep the speed
                        // do nothing
                    }
                }

                if (turnSteeringOn)
                {
                    double xChange = skeleton.Joints[JointType.HandRight].Position.X - handRightLastX;
                    double xChangeHandRight = skeleton.Joints[JointType.HandRight].Position.X - handRightLastX;
                    double xChangeHandLeft = skeleton.Joints[JointType.HandLeft].Position.X - handRightLastX;
                    double handDiffY = skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.HandLeft].Position.Y;


                    if (Math.Abs(handDiffY) < TURN_SNAP_TO_ZERO_HANDS_POS_DIF)
                    {
                        // snap to zero
                        X.Value = 0.0;
                    }
                    else
                    {
                        xChange *= TURN_SCALE;
                        if (skeleton.Joints[JointType.HandLeft].Position.Y >= skeleton.Joints[JointType.HandRight].Position.Y)
                        {
                            // turn right
                            X.ChangeValue(-xChange);
                        }
                        else
                        {
                            // turn left
                            X.ChangeValue(xChange);
                        }
                    }
                }
            }

            if (CheckStop(skeleton))
            {
                if (CheckMenu(skeleton))    // menu gesture contains the stop gesture
                {
                    if (MenuIssued != null)
                    {
                        MenuIssued(this, null);
                    }
                    return;
                }
                else  // not menu, just stop
                {
                    if (StopIssued != null)
                    {
                        StopIssued(this, null);
                    }
                }
            }

            UpdateSteeringHandPositions(skeleton);
            firstFrame = false;
        }

        /// <summary>
        /// Check if the main steering should be enabled.
        /// It will be when the left hand is below the left hip.
        /// </summary>
        /// <param name="skeleton">Postions of operator's body parts.</param>
        /// <returns>True if the main steering should be enabled.</returns>
        protected override bool CheckMainSteering(Skeleton skeleton)
        {
            return (CheckSpeedSteering(skeleton) || CheckTurnSteering(skeleton));
        }

        /// <summary>
        /// Check if the secondary steering should be enabled.
        /// It will be when the left hand is in space between the hips (actually past the left hip)
        /// and below shoulders.
        /// </summary>
        /// <param name="skeleton">Postions of operator's body parts.</param>
        /// <returns>True if the secondary steering should be enabled.</returns>
        protected override bool CheckSecondarySteering(Skeleton skeleton)
        {
            return CheckForkSteering(skeleton);
        }

        /// <summary>
        /// This should check if the operator made a gesture indicating that all controls
        /// should be restored to their defaults (stop the forklift).
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the forklift should stop.</returns>
        protected override bool CheckStop(Skeleton skeleton)
        {
            SkeletonPoint left = skeleton.Joints[JointType.HandLeft].Position;
            SkeletonPoint right = skeleton.Joints[JointType.HandRight].Position;

            // both hands above shoulders
            if (left.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y
                && right.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y)
            {
                // hands touching
                if (OperatorSkeleton.GetDistance(left, right) <= JOINT_TOUCH_RANGE)
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// This should check if the operator made a gesture indicating that application
        /// should return to menu. HUD will still be working but will be hidden.
        /// Also, the forklift should stop.
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the application should return to menu.</returns>
        protected override bool CheckMenu(Skeleton skeleton)
        {
            // first, the condition from stop needs to be met
            if (CheckStop(skeleton))
            {
                double dist = OperatorSkeleton.GetDistance(skeleton.Joints[JointType.ElbowLeft].Position,
                    skeleton.Joints[JointType.ElbowRight].Position);

                // if the elbows are touching
                if (dist <= JOINT_TOUCH_RANGE)
                    return true;
            }
            return false;
        }

        /// <summary>
        /// This should check if operate made gesture indicating that simulated 
        /// robot should speed up or slow down
        /// </summary>
        /// <param name="skeleton">Operator's skeleton</param>
        /// <returns></returns>
        private bool CheckSpeedSteering(Skeleton skeleton)
        {
            double diffX = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HandRight].Position.X);
            double diffY = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.HandRight].Position.Y);
            double diffZ = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Z - skeleton.Joints[JointType.HandRight].Position.Z);
            double diffXY = Math.Sqrt(Math.Pow(diffX, 2) + Math.Pow(diffY, 2));

            if (diffX <= SPEED_STEERING_HANDS_POS_DIFF_MAX_X && diffX >= SPEED_STEERING_HANDS_POS_DIFF_MIN_X &&
                diffY <= SPEED_STEERING_HANDS_POS_DIFF_MAX_YZ && diffY >= SPEED_STEERING_HANDS_POS_DIFF_MIN_YZ &&
                diffZ <= SPEED_STEERING_HANDS_POS_DIFF_MAX_YZ && diffZ >= SPEED_STEERING_HANDS_POS_DIFF_MIN_YZ)
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// This should check if operate made gesture indicating that simulated 
        /// robot should turn
        /// </summary>
        /// <param name="skeleton"></param>
        /// <returns></returns>
        private bool CheckTurnSteering(Skeleton skeleton)
        {
            double diffX = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HandRight].Position.X);
            double diffY = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.HandRight].Position.Y);
            double diffZ = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Z - skeleton.Joints[JointType.HandRight].Position.Z);
            double diffXY = Math.Sqrt(Math.Pow(diffX, 2) + Math.Pow(diffY, 2));

            if (diffXY <= TURN_STEERING_HANDS_POS_DIFF_MAX_XY && diffXY >= TURN_STEERING_HANDS_POS_DIFF_MIN_XY &&
                diffZ <= TURN_STEERING_HANDS_POS_DIFF_MAX_Z && diffZ >= TURN_STEERING_HANDS_POS_DIFF_MIN_Z)
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// This should check if operate made gesture indicating that simulated 
        /// robot should be able to move, turn or fork lift
        /// </summary>
        /// <param name="skeleton"></param>
        /// <returns></returns>
        private bool CheckForkSteering(Skeleton skeleton)
        {
            double diffX = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HandRight].Position.X);
            double diffY = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.HandRight].Position.Y);
            double diffZ = Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Z - skeleton.Joints[JointType.HandRight].Position.Z);

            if (diffX <= FORK_STEERING_HANDS_POS_DIFF_MAX_X && diffX >= FORK_STEERING_HANDS_POS_DIFF_MIN_X &&
                diffY <= FORK_STEERING_HANDS_POS_DIFF_MAX_YZ && diffY >= FORK_STEERING_HANDS_POS_DIFF_MIN_YZ &&
                diffZ <= FORK_STEERING_HANDS_POS_DIFF_MAX_YZ && diffZ >= FORK_STEERING_HANDS_POS_DIFF_MIN_YZ)
            {
                return true;
            }
            return false;
        }

    }
}
